import pygame
from pygame import joystick
import magnet_plate_robot as mrobot
import time
import threading

CLOCK = pygame.time.Clock()
FPS   = 15
ROBOT:mrobot.Magnet_Plate_Robot = None
SIZE  = [480, 240]
SPD   = [0., 0.]
POS   = [0., 0.]
AXIS_LIST  = [0, 1]
PARAMETERS = [[[-1, 1], [100, -100]], [[-1, 1], [100, -100]]]
SHIFT_CANCELLING_VALUE = 0.2
JOYSTICK:joystick.JoystickType = None


def spd_map_func(map_in:list=[-1,1], map_out:list=[-3600, 3600]):
    """将速度从区间a线性映射到区间b, 返回函数的斜率k和偏置b"""
    k = (map_out[1] - map_out[0]) / (map_in[1] - map_in[0])
    b = (map_out[0] - k*map_in[0])
    return k, b

def spd_map_func_(maps:list=[-1, 1, -3600, 3600]):
    """将速度从区间a线性映射到区间b, 返回函数的斜率k和偏置b"""
    k = (maps[3] - maps[2]) / (maps[1] - maps[0])
    b = (maps[2] - k*maps[0])
    return k, b

def axis_shift_cancelling(axis_value:float, fixValue:float)->float:
    """手柄的漂移补偿`axis_value ~ [-1, 1]`"""
    if fixValue > axis_value  > -fixValue:
        axis_value  = 0.
    elif -1. < axis_value  < -1. + fixValue:
        axis_value  = -1.
    elif 1. > axis_value  > 1. - fixValue:
        axis_value  = 1.
    return axis_value


_thred_run = True

def screen_print(suf:pygame.Surface, texts:str, 
                font:pygame.font.Font, inity=0):
    """使用该函数在屏幕上打印文字"""
    y = inity
    for text in texts.split("\n"):
        text_surface = font.render(text, True, (0, 0, 0))
        suf.blit(text_surface, (0, y))
        y += font.get_height()


def render(surf:pygame.Surface, font):
    surf.fill("white")
    screen_print(surf, 
    f"Speed of robot: [{SPD[0]}, {SPD[1]}]\nPos of robot: [{POS[0]:.2f}, {POS[1]:.2f}]",
    font)
    pygame.display.flip()
    CLOCK.tick(FPS)



def func_for_show_joysticks():
    joys = [joystick.Joystick(i).get_name() for i in range(joystick.get_count())]
    if joys:
        print("选择希望连接到的手柄")
        for k, joy in enumerate(joys):
            print(f"{k}: {joy}")
        return int(input("输入编号选择你想要连接的手柄: "))
    else:
        raise Exception("系统无法找到可用的手柄")

def event_handler():
    global SPD, _thred_run, JOYSTICK
    SPD = [0, 0]
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            _thred_run = False
            exit()
        elif event.type == pygame.JOYDEVICEREMOVED:
            print(f"Joystick {event.instance_id} disconnected")
            exit()
        elif event.type == pygame.KEYDOWN:
            if event.key == pygame.K_UP:
                SPD[0] += 100
            if event.key == pygame.K_DOWN:
                SPD[0] -= 100
            if event.key == pygame.K_LEFT:
                SPD[1] += 100
            if event.key == pygame.K_RIGHT:
                SPD[1] -= 100
        elif event.type == pygame.KEYUP:
            if event.key == pygame.K_UP:
                SPD[0] -= 100
            if event.key == pygame.K_DOWN:
                SPD[0] += 100
            if event.key == pygame.K_LEFT:
                SPD[1] -= 100
            if event.key == pygame.K_RIGHT:
                SPD[1] += 100
    
    for key, (axis, parameters) in enumerate(zip(AXIS_LIST, PARAMETERS)):
        joy_value = JOYSTICK.get_axis(axis)
        joy_value = axis_shift_cancelling(joy_value, SHIFT_CANCELLING_VALUE)
        k, b, = spd_map_func(*parameters)
        SPD[key] += (k * joy_value) + b


def pos_getter(interval=1):
    global POS, ROBOT
    while _thred_run:
        POS[0], POS[1] = ROBOT.get_position()
        time.sleep(interval)

def main():
    global ROBOT, JOYSTICK
    ser_lock = threading.Lock()
    pygame.init()
    pygame.font.init()
    try:
        ...
        ROBOT = mrobot.Magnet_Plate_Robot(lock=ser_lock)
    except Exception as e:
        input(f"机器人启动失败, 可能是未检测到串口连接.错误信息：\n{e}")
        exit()
    try:
        # pygame.joystick.init()
        JOYSTICK = joystick.Joystick(func_for_show_joysticks())
        JOYSTICK.init()
    except Exception as e:
        input(f"手柄连接失败.错误信息：\n{e}")
        exit()

    font = pygame.font.SysFont(name="arial", size=35)
    display = pygame.display.set_mode(SIZE, pygame.RESIZABLE)
    # threading.Thread(target=pos_getter).start()
    pygame.display.set_caption(f"robot at {ROBOT.ser.port}")
    while True:
        render(display, font)
        event_handler()
        ROBOT.set_speed(*SPD)
        # POS[0], POS[1] = ROBOT.get_position()

if __name__ == "__main__":
    main()